`include "c3d_timescale.v"
`include "c3d_defines.v"

module calc_core(
    input wire rst,
    input wire clk,
    input wire [63:0] ins,
    input wire signed [15:0] xpos,
    input wire signed [15:0] ypos,
    input wire signed [15:0] x[2:0],
    input wire signed [15:0] y[2:0],
    input wire busy,
    output reg mask
);

// sint32 16(integer)16(frac)

// ins: type(8) tar(6) sat(1) condition(1) src(index(6),order(8),abs(1),neg(1))(3)

reg signed [3:0][31:0] regfile[63:0];// SIMD

wire signed [3:0][31:0] res;
reg signed [3:0][31:0] res_ori;

wire signed [3:0][31:0] src[2:0];
wire signed [3:0][31:0] src_abs[2:0];
wire signed [3:0][31:0] src_order[2:0];
wire signed [3:0][31:0] src_index[2:0];

genvar i, j;

generate
    for (i = 0; i < 3; i = i + 1) begin : shuffle
        assign src_index[i] = regfile[ins[(i << 4) + 21 : (i << 4) + 16]];
        assign src_order[i] = {src_index[i][ins[(i << 4) + 29 : (i << 4) + 28]],
                               src_index[i][ins[(i << 4) + 27 : (i << 4) + 26]],
                               src_index[i][ins[(i << 4) + 25 : (i << 4) + 24]],
                               src_index[i][ins[(i << 4) + 23 : (i << 4) + 22]]};
        assign src_abs[i] = ins[(i << 4) + 30] ? {(src_order[i][3] < 32'd0 ? src_order[i][3] : 32'd0),
                                                  (src_order[i][2] < 32'd0 ? src_order[i][2] : 32'd0),
                                                  (src_order[i][1] < 32'd0 ? src_order[i][1] : 32'd0),
                                                  (src_order[i][0] < 32'd0 ? src_order[i][0] : 32'd0)} : src_order[i];
        assign src[i] = ins[(i << 4) + 31] ? {-src_abs[i][3], -src_abs[i][2], -src_abs[i][1], -src_abs[i][0]} : src_abs[i];
    end
endgenerate

wire [31:0] idx_str[2:0];

generate
    for (i = 0; i < 3; i = i + 1) begin : idxX
        for (j = 0; j < 4; j = j + 1) begin : idxY
            assign idx_str[i][(j << 3) + 7 : (j << 3)] = 
                (ins[(i << 4) + (j << 1) + 23] ?
                (ins[(i << 4) + (j << 1) + 22] ? "w" : "z") :
                (ins[(i << 4) + (j << 1) + 22] ? "y" : "x"));
        end
    end
endgenerate

wire signed [31:0] g[2:0];
wire valid;

assign g[0] = (x[2] - xpos) * (y[1] - ypos) - (x[1] - xpos) * (y[2] - ypos);
assign g[1] = (x[0] - xpos) * (y[2] - ypos) - (x[2] - xpos) * (y[0] - ypos);
assign g[2] = (x[1] - xpos) * (y[0] - ypos) - (x[0] - xpos) * (y[1] - ypos);

assign valid = ((g[0] >= 32'sd0) && (g[1] >= 32'sd0) && (g[2] >= 32'sd0));

reg regout, sat;

generate
    for (i = 0; i < 4; i = i + 1) begin : satLayer
        assign res[i] = (sat ? (res_ori[i] < 32'sd0 ? 32'sd0 : (res_ori[i] > 32'sh10000 ? 32'sh10000 : res_ori[i])) : res_ori[i]);
    end
endgenerate

parameter STATE_IDLE = 1'b0,
          STATE_RUN = 1'b1;
reg state;

reg [5:0] index;

always @(posedge clk or negedge rst) begin
    if (!rst) begin
        mask <= 1'b0;
        state <= STATE_IDLE;
    end
    else begin
        case (state)
        STATE_IDLE: begin
            if (busy) begin
                mask <= valid;
                if (valid) begin
                    regout <= 1'b0;
                    state <= STATE_RUN;
                    // 测试
                    regfile[0][0] <= g[0];
                    regfile[0][1] <= g[1];
                    regfile[0][2] <= g[2];
                    regfile[0][3] <= 32'sh10000;
                    regfile[1][0] <= 32'sh10000;
                    regfile[1][1] <= 32'sh10000;
                    regfile[1][2] <= 32'sh10000;
                    regfile[1][3] <= 32'sh10000;
                end
            end
        end
        STATE_RUN: begin
            index <= ins[13:8];
            if (regout) begin
                regfile[index] <= res;
            end
            if (!busy) begin
                state <= STATE_IDLE;
            end
            else begin
                regout <= 1'b1;
                sat <= ins[14];
                case (ins[7:0])
                8'h00: $display("Core %d %d nop", xpos, ypos);
                8'h01: begin
                    $display("Core %d %d add r%02d,r%02d.%s,r%02d.%s", xpos, ypos, ins[13:8], ins[21:16], idx_str[0], ins[37:32], idx_str[1]);
                    res_ori <= {
                        src[0][0] + src[1][0],
                        src[0][1] + src[1][1],
                        src[0][2] + src[1][2],
                        src[0][3] + src[1][3]
                    };
                end
                8'h02: begin
                    $display("Core %d %d sub r%02d,r%02d.%s,r%02d.%s", xpos, ypos, ins[13:8], ins[21:16], idx_str[0], ins[37:32], idx_str[1]);
                    res_ori <= {
                        src[0][0] - src[1][0],
                        src[0][1] - src[1][1],
                        src[0][2] - src[1][2],
                        src[0][3] - src[1][3]
                    };
                end
                8'h03: begin
                    $display("Core %d %d mul r%02d,r%02d.%s,r%02d.%s", xpos, ypos, ins[13:8], ins[21:16], idx_str[0], ins[37:32], idx_str[1]);
                    res_ori <= {
                        (src[0][0] * src[1][0]) >>> 16,
                        (src[0][1] * src[1][1]) >>> 16,
                        (src[0][2] * src[1][2]) >>> 16,
                        (src[0][3] * src[1][3]) >>> 16
                    };
                end
                8'h04: begin
                    $display("Core %d %d div r%02d,r%02d.%s,r%02d.%s", xpos, ypos, ins[13:8], ins[21:16], idx_str[0], ins[37:32], idx_str[1]);
                    res_ori <= {
                        (src[0][0] <<< 16) / src[1][0],
                        (src[0][1] <<< 16) / src[1][1],
                        (src[0][2] <<< 16) / src[1][2],
                        (src[0][3] <<< 16) / src[1][3]
                    };
                end
                8'h05: begin
                    $display("Core %d %d def r%02d.%c,%f", xpos, ypos, ins[13:8], idx_str[0][7:0], ins[47:16] / 65536.0);
                    res_ori <= regfile[ins[13:8]];
                    res_ori[ins[15:14]] <= ins[47:16];
                    sat <= 1'b0;
                end
                default: begin
                    $display("Core %d %d #UD %x", xpos, ypos, ins[7:0]);
                    regout <= 1'b0;
                    sat <= 1'b0;
                end
                endcase
                // 测试
                $display("r0 x:%f y:%f z:%f w:%f", regfile[0][0] / 65536.0, regfile[0][1] / 65536.0, regfile[0][2] / 65536.0, regfile[0][3] / 65536.0);
                $display("r1 x:%f y:%f z:%f w:%f", regfile[1][0] / 65536.0, regfile[1][1] / 65536.0, regfile[1][2] / 65536.0, regfile[1][3] / 65536.0);
            end
        end
        endcase
    end
end

endmodule